package com.iamteer.cupband;

/**
 * Created by Chuck on 15/8/25.
 */
public class ComputeAngle {
    private byte[] rawdata = new byte[8];
    public class GSensor{
        public short x;
        public short y;
        public short z;
    }

    public String setRawData(byte[] rawData){
        this.rawdata = rawData;
        int i;
        String hexString = "";
        for(i=0;i< this.rawdata.length;i++){
            hexString += Integer.toHexString((this.rawdata[i] & 0x000000FF) | 0xFFFFFF00).substring(6).toUpperCase();
            hexString += " ";
        }
        return hexString;
    }

    public GSensor getAcceleration(){
        GSensor gSensor = new GSensor();

        int a, b, c;
        a = ((char) rawdata[0]&0xFF) + (((char)rawdata[1]&0xFF) << 8);
        b = ((char) rawdata[2]&0xFF) + (((char)rawdata[3]&0xFF) << 8);
        c = ((char) rawdata[4]&0xFF) + (((char)rawdata[5]&0xFF) << 8);

//        System.out.println(String.format("x: %d,\t y: %d,\t z: %d", a, b, c ));

        gSensor.x = (short) (a < 32768 ? a: a-65536);
        gSensor.y = (short) (b < 32768 ? b: b-65536);
        gSensor.z = (short) (c < 32768 ? c: c-65536);


        return gSensor;
    }

    public byte getLastLevel(){
        return rawdata[7];
    }

    public double getAngle(short z, short y){
        double res;
        res = Math.atan2(z, -y);
        return res*180/Math.PI;
    }

}
